//
// Created by xujingyi on 2021/5/21.
//

#ifndef ALLIANCE_SENTRY_V2_0_PREDICTOR_H
#define ALLIANCE_SENTRY_V2_0_PREDICTOR_H
#include "opencv2/video/tracking.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include "find/armor_class.h"
#include "Serial.h"
namespace rm{
    typedef enum {
        UNKNOW, TRACK_ARMOR=1, CHANGE_ARMOR=2, CHANGE_ENEMY=3, TRACK_GYRO=4     //自瞄状态枚举定义
    } ArmorState;
    typedef enum{
        UNIFORM=1,SHAKE_MODE=2,SUDDEN_MOVE=3
    }ChassisState;

    class KalmanFilter {
    public:
        KalmanFilter(int mode);
        KalmanFilter(bool s);
        void init(int mode);
        void init(bool s);

        cv::Point2f p1;
        cv::Point2f p2;
        cv::Point3f p3f1,p3f2;
        cv::KalmanFilter *Kf;

        cv::Point3f predict(const ChassisState chassis_state,cv::Point3f mes,int n=0,const float delay_t=0);
        cv::Mat predict(cv::Mat mes,int n=0,const float delay_t=0);
        cv::Point3f predict(const ArmorState armor_state,cv::Point3f coord,int n=0,const float delay_t=0);
    private:
        cv::Point3f _vel,_pos;
        int _mode=-1;
        int _change_armor_cnt=0;
    };

    class Predictor{
    public:
        std::vector<rm::BoxPos> Boxposes;
        cv::Point2f last_no_cmpns_pre_cen;
        const int posSize=40;
#if PRE_MODE==PIXEL_MODE
        Predictor(){
            KFy = new KalmanFilter(1);//yaw
            KFp = new KalmanFilter(2);//pitch
            UltimateKFy = new KalmanFilter(true);//dx
            UltimateKFp = new KalmanFilter(true);//dy
        }
        void init(){
            KFy->init(1);//yaw
            KFp ->init(2);//pitch
            UltimateKFy ->init(true);//dx
            UltimateKFp ->init(true);//dy
        }
        cv::Point2f operator()(const ArmorState armor_state,const ChassisState chassis_state,cv::Point2f center,const float distance,const float delay_t=0);
        KalmanFilter *KFy;
        KalmanFilter *KFp;
        KalmanFilter *UltimateKFy;
        KalmanFilter *UltimateKFp;
#elif PRE_MODE==COORD_MODE
        Predictor(){
            KFl=new KalmanFilter(3);//chassis_location
            KFc=new KalmanFilter(4);//world coordination
            KFy=new KalmanFilter(5);//
            KFp=new KalmanFilter(6);//
        }
        void init(){
            Boxposes.clear();
            KFl ->init(3);
            KFc ->init(4);//world coordination
            KFy ->init(5);//R_vec
            KFp ->init(6);//R_vec
        }

        cv::Point2f operator()(const ArmorState armor_state,const ChassisState chassis_state,cv::Point3f world_coord,const float delay_t=0);

        KalmanFilter *KFl;
        KalmanFilter *KFc;
        KalmanFilter *KFp;
        KalmanFilter *KFy;
#endif
        ~Predictor()=default;
    };

/*
 * 小陀螺预测类，yaw轴角度，yaw轴角速度
 */
//    class GyroPredictor{
//    public:
//        double pre_pos=-1;
//        double v;
//        cv::Mat measurement;
//        GyroPredictor()= default;
//        GyroPredictor(float trustm);
//        ~ GyroPredictor()= default;
//        cv::KalmanFilter kf;
//        /*!
//         * 修正kalman
//         * @param state 当前测量状态
//         */
//        void correct(double angle);
//        /*!
//         * @param t 预测t（秒）时间后的目标位置
//         */
//        double predict(float angle, int n,int delta_t);
//    };
    class GyroKalmanFilter {
    public:
//        GyroKalmanFilter()= default;
//        ~ GyroKalmanFilter()= default;
        GyroKalmanFilter(int mode);
        void init(int mode);

        cv::Point2f p1;
        cv::Point2f p2;
        cv::KalmanFilter *Kf;

        cv::Point2f predict(cv::Point2f mes,int n,float delay_t);
        cv::Mat predict(cv::Mat mes);
    };

    class GyroPredictor{
    public:
        ~ GyroPredictor()= default;
        GyroPredictor();
        void init();

        double pre_pos=-1;
        float last_pre_x;
        double v;
        cv::Mat measurement;
        cv::KalmanFilter kf;//gyro angle
        GyroKalmanFilter *KFy;//motor_yaw
        GyroKalmanFilter *UltimateKFy;//yaw
        /*!
         * 修正kalman
         * @param state 当前测量状态
         */
        void correct(double angle);
        /*!
         * @param t 预测t（秒）时间后的目标位置
         */
        cv::Point2f predict(float pixel_x,float angle,int n,int delay_t);
    };
}

#endif //ALLIANCE_SENTRY_V2_0_PREDICTOR_H
